/**
 * @ Author: luoqi
 * @ Create Time: 2024-10-31 20:26
 * @ Modified by: luoqi
 * @ Modified time: 2025-04-11 19:30
 * @ Description:
 */

#include <bsp.h>
#include <stdbool.h>
#include <string.h>
#include "link.h"
#include "../qshell/qterm.h"
#include "../dev/qdrive.h"

static QDrive *drv;

static QCliCmd cmd_link;
static int cmd_link_hdl(int argc, char **argv);

static TX_THREAD task_downlink;
static uint8_t task_downlink_stack[1024];

static TX_THREAD task_uplink;
static uint8_t task_uplink_stack[1024];

static bool isdbg = false;

static TX_EVENT_FLAGS_GROUP event_can1;
#define _CAN_RX_EVENT (1 << 0)
#define _CAN_TX_EVENT (1 << 1)

static CAN_TxHeaderTypeDef can_tx;
static CAN_RxHeaderTypeDef can_rx;

typedef struct {
    int ecode;
    uint32_t id;
    uint8_t data[8];
} CanMsg;

static CanMsg can_msg;

static int can_send(uint32_t id, const uint8_t *data, uint8_t len)
{
    if(len > 8) {
        return -1;
    }
    can_tx.StdId = id;
    can_tx.IDE = CAN_ID_STD;
    can_tx.ExtId = 0;
    can_tx.RTR = CAN_RTR_DATA;
    can_tx.DLC = len;
    can_tx.TransmitGlobalTime = DISABLE;

    uint32_t tx_mbox;

    return HAL_CAN_AddTxMessage(&hcan1, &can_tx, data, &tx_mbox);
}

static void can_filter_init(uint8_t id)
{
    CAN_FilterTypeDef filter;
    filter.FilterBank = 0;
    filter.FilterMode = CAN_FILTERMODE_IDMASK;
    filter.FilterScale = CAN_FILTERSCALE_32BIT;
    filter.FilterIdHigh = 0x0000;
    filter.FilterIdLow = 0x0000 | (id & 0x7f);
    filter.FilterMaskIdHigh = 0x0700;
    filter.FilterMaskIdLow = 0xffff;
    filter.FilterFIFOAssignment = CAN_RX_FIFO0;
    filter.FilterActivation = ENABLE;
    filter.SlaveStartFilterBank = 0;
    if(HAL_CAN_ConfigFilter(&hcan1, &filter) != HAL_OK) {
        QSH(" #! can filter config failed !\r\n");
    }
}

static int can_recv(uint32_t *id, uint8_t *rdata)
{
    uint32_t flags;
    tx_event_flags_get(&event_can1, _CAN_RX_EVENT, TX_OR_CLEAR, &flags, TX_WAIT_FOREVER);
    memcpy(rdata, can_msg.data, 8);
    *id = can_msg.id;
    return can_msg.ecode;
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan->Instance == CAN1) {
        can_msg.ecode = HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &can_rx, can_msg.data);
        can_msg.id = can_rx.StdId;
        tx_event_flags_set(&event_can1, _CAN_RX_EVENT, TX_OR);
    }
}

static void task_downlink_hdl(uint32_t args)
{
    UNUSED(args);
    can_filter_init(drv->link_id);
    if(HAL_CAN_Start(&hcan1) != HAL_OK) {
        QSH(" #! can start failed !\r\n");
    }
    if(HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
        QSH(" #! can filter active notification failed !\r\n");
    }
    uint32_t id;
    uint8_t rdata[8];
    while(1) {
        if(can_recv(&id, rdata) == HAL_OK) {
            id = id & 0xff;
            switch(id) {
            case QDRV_LINK_CMD_SET_FOC_RMOD:
                break;
            case QDRV_LINK_CMD_SET_FOC_CMOD:
                break;
            case QDRV_LINK_CMD_SET_FOC_I:
                break;
            case QDRV_LINK_CMD_SET_FOC_V:
                break;
            case QDRV_LINK_CMD_SET_FOC_P:
                break;
            case QDRV_LINK_CMD_SET_FOC_IMAX:
                break;
            case QDRV_LINK_CMD_SET_FOC_VMAX:
                break;
            case QDRV_LINK_CMD_SET_FOC_FB_FRQ:
                break;
            case QDRV_LINK_CMD_SET_FOC_FB_TYPE:
                break;
            case QDRV_LINK_CMD_SET_FOC_FB_DEV:
                break;
            case QDRV_LINK_CMD_SET_FOC_ENABLE:
                break;
            case QDRV_LINK_CMD_SET_FOC_STOP:
                break;
            default:
                break;
            }
        } else {
            continue;
        }
    }
}

static void task_uplink_hdl(uint32_t args)
{
    UNUSED(args);
    uint8_t data[8];
    QDrvFbId id;
    while(1) {
        if(drv->ecode != QDRV_ERR_NONE) {
            can_send(((uint32_t)drv->link_id << 3), (uint8_t *)&drv->ecode, 4);
            tx_thread_sleep(500);
            continue;
        }

        if(drv->link_fb_frq != 0) {
            switch(drv->link_fb_type) {
            case QDRV_FB_TYPE_I:
                id = QDRV_FB_I;
                if(drv->link_fb_dev == QDRV_FB_DEV_ALL) {
                    memset(data, drv->foc1.iq, 4);
                    memset(data + 4, drv->foc2.iq, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 8);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC1) {
                    memset(data, drv->foc1.iq, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC2) {
                    memset(data, drv->foc2.iq, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else {
                    continue;
                }
                break;
            case QDRV_FB_TYPE_V:
                id = QDRV_FB_V;
                if(drv->link_fb_dev == QDRV_FB_DEV_ALL) {
                    memset(data, drv->foc1.vel, 4);
                    memset(data + 4, drv->foc2.vel, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 8);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC1) {
                    memset(data, drv->foc1.vel, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC2) {
                    memset(data, drv->foc2.vel, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else {
                    continue;
                }
                break;
            case QDRV_FB_TYPE_P:
                id = QDRV_FB_P;
                if(drv->link_fb_dev == QDRV_FB_DEV_ALL) {
                    memset(data, drv->foc1.pos, 4);
                    memset(data + 4, drv->foc2.pos, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 8);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC1) {
                    memset(data, drv->foc1.pos, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else if(drv->link_fb_dev == QDRV_FB_DEV_FOC2) {
                    memset(data, drv->foc2.pos, 4);
                    can_send(((uint32_t)drv->link_id << 3) | id, data, 4);
                } else {
                    continue;
                }
                break;
            default:
                break;
            }
            tx_thread_sleep(1000 / drv->link_fb_frq);
        } else {
            tx_thread_sleep(200);
        }
    }
}

int link_init(void)
{
    drv = qdrv_get();
    tx_event_flags_create(&event_can1, "events_can1");
    tx_event_flags_set(&event_can1, 0, TX_AND);
    tx_thread_create(&task_downlink, "downlink", task_downlink_hdl, 0, task_downlink_stack, sizeof(task_downlink_stack), 5, 5, TX_NO_TIME_SLICE, TX_AUTO_START);
    tx_thread_create(&task_uplink, "uplink", task_uplink_hdl, 0, task_uplink_stack, sizeof(task_uplink_stack), 5, 5, TX_NO_TIME_SLICE, TX_AUTO_START);
    qterm_attach(&cmd_link, "link", cmd_link_hdl, "can bus link commands");
    return 0;
}

static int args_dbg_hdl(int argc, char **argv)
{
    UNUSED(argc);
    if(ISARG(argv[1], "on")) {
        isdbg = true;
    } else if(ISARG(argv[1], "off")) {
        isdbg = false;
    } else {
        QSH(" -dbg <on/off>\r\n");
    }
    return 0;
}

static QCliArgsEntry args_dbg_table[] = {
    {"dbg", 2, 2, args_dbg_hdl, "<off/on>"},
};

int cmd_link_hdl(int argc, char **argv)
{
    if(ISARG(argv[1], "?") && argc == 2) {
        return qterm_help(args_dbg_table, sizeof(args_dbg_table));
    } 
    return qcli_args_handle(argc, argv, args_dbg_table, sizeof(args_dbg_table));
}
